{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Extended Kalman Filter\n",
    "\n",
    "Extended Kalman filter on a moving 2D point, but done using factor graphs.\n",
    "This example uses the ExtendedKalmanFilter class to perform filtering\n",
    "on a linear system, demonstrating the same operations as in elaboratePoint2KalmanFilter.\n",
    "\n",
    "Author: Matt Kielo"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "source": [
    "GTSAM Copyright 2010-2022, Georgia Tech Research Corporation,\n",
    "Atlanta, Georgia 30332-0415\n",
    "All Rights Reserved\n",
    "\n",
    "Authors: Frank Dellaert, et al. (see THANKS for the full author list)\n",
    "\n",
    "See LICENSE for the license information"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/python/gtsam/examples/easyPoint2KalmanFilter.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {
    "tags": [
     "remove-cell"
    ]
   },
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Note: you may need to restart the kernel to use updated packages.\n"
     ]
    }
   ],
   "source": [
    "%pip install --quiet gtsam gtbook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "import gtsam\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create the Kalman Filter initialization point\n",
    "X0 = gtsam.Point2(0.0, 0.0)\n",
    "P0 = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]))\n",
    "\n",
    "# Create Key for initial pose\n",
    "x0 = gtsam.symbol('x', 0)\n",
    "\n",
    "# Create an ExtendedKalmanFilter object\n",
    "ekf = gtsam.ExtendedKalmanFilterPoint2(x0, X0, P0)\n",
    "\n",
    "# For this example, we use a constant-position model where\n",
    "# controls drive the point to the right at 1 m/s\n",
    "# F = [1 0; 0 1], B = [1 0; 0 1], and u = [1; 0]\n",
    "# Process noise Q = [0.1 0; 0 0.1]\n",
    "Q = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1]), True)\n",
    "\n",
    "# Measurement noise, assuming a GPS-like sensor\n",
    "R = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.25, 0.25]), True)\n",
    "\n",
    "# Motion model - move right by 1.0 units\n",
    "dX = gtsam.Point2(1.0, 0.0)\n",
    "\n",
    "last_symbol = x0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "X1 Predict: [1. 0.]\n",
      "X1 Update: [1. 0.]\n",
      "X2 Predict: [2. 0.]\n",
      "X2 Update: [2. 0.]\n",
      "X3 Predict: [3. 0.]\n",
      "X3 Update: [3. 0.]\n",
      "\n",
      "Easy Final Covariance (after update):\n",
      " [[0.01930567 0.        ]\n",
      " [0.         0.01930567]]\n"
     ]
    }
   ],
   "source": [
    "for i in range(1, 4):\n",
    "    # Create symbol for new state\n",
    "    xi = gtsam.symbol('x', i)\n",
    "    \n",
    "    # Prediction step: P(x_i) ~ P(x_i|x_{i-1}) P(x_{i-1})\n",
    "    # In Kalman Filter notation: x_{t+1|t} and P_{t+1|t}\n",
    "    motion = gtsam.BetweenFactorPoint2(last_symbol, xi, dX, Q)\n",
    "    Xi_predict = ekf.predict(motion)\n",
    "    print(f\"X{i} Predict:\", Xi_predict)\n",
    "    \n",
    "    # Update step: P(x_i|z_i) ~ P(z_i|x_i)*P(x_i)\n",
    "    # Assuming a measurement model h(x_{t}) = H*x_{t} + v\n",
    "    # where H is the observation model/matrix and v is noise with covariance R\n",
    "    measurement = gtsam.Point2(float(i), 0.0)\n",
    "    meas_factor = gtsam.PriorFactorPoint2(xi, measurement, R)\n",
    "    Xi_update = ekf.update(meas_factor)\n",
    "    print(f\"X{i} Update:\", Xi_update)\n",
    "    \n",
    "    # Move to next state\n",
    "    last_symbol = xi\n",
    "\n",
    "A = ekf.Density().getA()\n",
    "information_matrix = A.transpose() @ A\n",
    "covariance_matrix = np.linalg.inv(information_matrix)\n",
    "print (\"\\nEasy Final Covariance (after update):\\n\", covariance_matrix)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "py312",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.12.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
